import { Point3 } from "../point/Point3";
import { Vector3 } from "../vector/Vector3";
import { XYZ } from "../coord/XYZ";
import { Vector4 } from "../vector/Vector4";

/**
* @description 4x4矩阵
* 行主序
*  1, 0, 0, 0
*  0, 1, 0, 0
*  0, 0, 1, 0
*  0, 0, 0, 1
* @author xuld
* @date 2024-11-11
* @lastEditTime 2024-11-11 09:46:27
* @lastEditors xuld
*/

export class Matrix4 {
    private _data: number[][];

    public constructor(data?: number[][]) {
        if (data) {
            //验证是否是4x4矩阵
            if (this.checkArray4(data)) {
                data.forEach(item => {
                    this.checkArray4(item);
                })
            }
            this._data = this.rawDeepClone(data);
        }
        else {
            //默认单位矩阵
            this._data = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]
        }
    }

    /**
     * @description 验证是否是4个元素的数组
     * @param item 被验证的数组
     * @returns true 是4个元素的数组，抛异常则不是
     */
    private checkArray4(item: any): boolean {
        if (item instanceof Array && item.length == 4) {
            return true
        } else {
            throw new Error("Parameter needs 4 array");
        }
    }

    /**
     * @description 获取数据第i行第j列的值
     * @param i - 行索引
     * @param j - 列索引
     * @returns 矩阵元素值
     */
    public get(i: number, j: number): number {
        if (i < 0 || i >= 4 || j < 0 || j >= 4) {
            throw new Error("Index out of bounds");
        }
        return this._data[i][j];
    }

    /**
     * @description 设置第i行第j列的值
     * @param i - 行索引
     * @param j - 列索引
     * @param value - 要设置的值
     */
    public set(i: number, j: number, value: number) {
        if (i < 0 || i >= 4 || j < 0 || j >= 4) {
            throw new Error("Index out of bounds");
        }
        this._data[i][j] = value;
    }

    /**
     * @description 获取原始数据二维数组
     * @returns 矩阵的原始数据数组
     */
    public get raw(): number[][] {
        return this._data;
    }

    /**
     * @description 获取单位矩阵
     * @returns 4x4单位矩阵
     */
    public static Identity(): Matrix4 {
        let mat: Matrix4 = new Matrix4();
        return mat;
    }

    /**
     * @description 获取零矩阵
     * @returns 4x4零矩阵
     */
    public static Zero(): Matrix4 {
        let mat: Matrix4 = new Matrix4();
        for (let i = 0; i < 4; i++)
            mat._data[i][i] = 0;
        return mat;
    }

    /**
    * @description 生成一个平移、旋转和缩放的4x4矩阵
    * 将对象置于位置 pos 处， 按旋转zyx进行定向，并且按 s 进行缩放。
    * @param pos - 位置
    * @param rots - 绕各轴的旋转角（弧度）
    * @param scale - 缩放
    *
    * @returns  Matrix4
    * @public
    */
    public static TRS(pos: Point3, rots: XYZ, scale: XYZ): Matrix4 {
        let mat1: Matrix4 = Matrix4.Translate(pos);
        let mat2: Matrix4 = Matrix4.Rotate(rots);
        let mat3: Matrix4 = Matrix4.Scale(scale);
        return mat3.multiplyMatrix(mat2).multiplyMatrix(mat1);
    }

    /**
    * @description 创建平移矩阵
    * @param pos 平移的位置坐标
    * @returns 平移矩阵
    * @public
    */
    public static Translate(pos: Point3): Matrix4 {
        let mat: Matrix4 = new Matrix4();
        mat._data[0][3] = pos.x;
        mat._data[1][3] = pos.y;
        mat._data[2][3] = pos.z;
        mat._data[3][3] = 1;
        return mat;
    }

    /**
    * @description 旋转矩阵
    * @param rots 绕各轴的旋转角（弧度），次序z=>y=\>x
    * @returns 旋转后矩阵
    */
    public static Rotate(rots: XYZ): Matrix4 {
        let mat: Matrix4 = new Matrix4();
        if (rots.x == 0 && rots.y == 0 && rots.z == 0) {
            mat._data[0][0] = 1;
            mat._data[1][1] = 1;
            mat._data[2][2] = 1;
        }
        else {
            let roll = rots.x;
            let pitch = rots.y;
            let yaw = rots.z;

            mat._data[0][0] = Math.cos(yaw) * Math.cos(pitch);
            mat._data[0][1] = Math.cos(yaw) * Math.sin(pitch) * Math.sin(roll) - Math.sin(yaw) * Math.cos(roll);
            mat._data[0][2] = Math.cos(yaw) * Math.sin(pitch) * Math.cos(roll) + Math.sin(yaw) * Math.sin(roll);
            mat._data[1][0] = Math.sin(yaw) * Math.cos(pitch);
            mat._data[1][1] = Math.sin(yaw) * Math.sin(pitch) * Math.sin(roll) + Math.cos(yaw) * Math.cos(roll);
            mat._data[1][2] = Math.sin(yaw) * Math.sin(pitch) * Math.cos(roll) - Math.cos(yaw) * Math.sin(roll);
            mat._data[2][0] = -Math.sin(pitch);
            mat._data[2][1] = Math.cos(pitch) * Math.sin(roll);
            mat._data[2][2] = Math.cos(pitch) * Math.cos(roll);
        }
        mat._data[3][3] = 1;

        return mat;
    }

    /**
     * @description 绕轴旋转
     * @param degrees 旋转的角度
     * @param axis 旋转的轴或方向
     * @param pivotPoint 一个用于确定旋转中心的点。默认(0,0,0)
     * @returns 旋转矩阵
     */
    public static RotateAxis(degrees: number, axis: XYZ, pivotPoint?: XYZ): Matrix4 {
        let tz = 0;
        let ty = tz;
        let tx = ty;
        if (pivotPoint) {
            tx = pivotPoint.x;
            ty = pivotPoint.y;
            tz = pivotPoint.z;
        }
        let rots = new XYZ(tx, ty, tz)
        let mat: Matrix4 = new Matrix4();

        if (rots.x == 0 && rots.y == 0 && rots.z == 0) {
            mat._data[0][0] = 1;
            mat._data[1][1] = 1;
            mat._data[2][2] = 1;
            mat._data[3][3] = 1;
        }

        let radian = degrees * Math.PI / 180;
        let cos = Math.cos(radian);
        let sin = Math.sin(radian);
        let x = axis.x;
        let y = axis.y;
        let z = axis.z;
        let x2 = x * x;
        let y2 = y * y;
        let z2 = z * z;
        let ls = x2 + y2 + z2;
        if (ls != 0) {
            let l = Math.sqrt(ls);
            x /= l;
            y /= l;
            z /= l;
            x2 /= ls;
            y2 /= ls;
            z2 /= ls;
        }
        let ccos = 1 - cos;

        let d = mat._data;
        d[0][0] = cos + ccos * x2;
        d[0][1] = x * y * ccos + z * sin;
        d[0][2] = x * z * ccos - y * sin;
        d[1][0] = x * y * ccos - z * sin;
        d[1][1] = cos + ccos * y2;
        d[1][2] = y * z * ccos + x * sin;
        d[2][0] = x * z * ccos + y * sin;
        d[2][1] = y * z * ccos - x * sin;
        d[2][2] = cos + ccos * z2;
        d[3][0] = (rots.x * (y2 + z2) - x * (rots.y * y + rots.z * z)) * ccos + (rots.y * z - rots.z * y) * sin;
        d[3][1] = (rots.y * (x2 + z2) - y * (rots.x * x + rots.z * z)) * ccos + (rots.z * x - rots.x * z) * sin;
        d[3][2] = (rots.z * (x2 + y2) - z * (rots.x * x + rots.y * y)) * ccos + (rots.x * y - rots.y * x) * sin;

        return mat
    }

    /**
    * @description 缩放矩阵
    * @param s 缩放的值
    * @returns 缩放矩阵
    */
    public static Scale(s: XYZ): Matrix4 {
        let mat: Matrix4 = new Matrix4();
        mat._data[0][0] = s.x;
        mat._data[1][1] = s.y;
        mat._data[2][2] = s.z;
        mat._data[3][3] = 1;
        return mat;
    }

    /**
    * @description 求任意平面的镜像矩阵,返回结果.
    * 思路：对法线向量归一 =>  若平面过原点,求该平面的镜像矩阵 => 若平面不过原点，计算偏移量 => 求出点到平面的垂直投影点*2的向量 =\>将偏移值添加至镜像矩阵
    * @param normal 平面法线
    * @param point 平面上的点
    * @returns 镜像矩阵
    */
    public static MirrorOfPlane(normal: Vector3, point: Point3 = new Point3(0, 0, 0)): Matrix4 {
        normal = normal.normalize()

        let normalX: number = normal.x
        let normalY: number = normal.y
        let normalZ: number = normal.z
        let powX: number = Math.pow(normal.x, 2)
        let powY: number = Math.pow(normal.y, 2)
        let powZ: number = Math.pow(normal.z, 2)
        let mat = new Matrix4()
        let pointX = point.x
        let pointY = point.y
        let pointZ = point.z

        let posX = 0
        let posY = 0
        let posZ = 0

        //不过原点
        if (normalX * pointX + normalY * pointY + normalZ * pointZ != 0) {
            let xyzPowSum = (normalX * normalX + normalY * normalY + normalZ * normalZ)
            let x = (normalX * normalY * pointY + normalX * normalZ * pointZ + normalX * normalX * pointX) / xyzPowSum
            let y = (normalY * normalZ * pointZ + normalY * normalX * pointX + normalY * normalY * pointY) / xyzPowSum
            let z = (normalX * pointX * normalZ + normalY * pointY * normalZ + normalZ * normalZ * pointZ) / xyzPowSum

            let newXYZ = new XYZ(x, y, z)
            newXYZ.multiplySelf(2)

            posX = newXYZ.x
            posY = newXYZ.y
            posZ = newXYZ.z
        }

        mat._data =
            [
                [powZ + powY - powX, -2 * normalX * normalY, -2 * normalX * normalZ, posX],
                [-2 * normalX * normalY, powX + powZ - powY, -2 * normalY * normalZ, posY],
                [-2 * normalX * normalZ, -2 * normalY * normalZ, powY + powX - powZ, posZ],
                [0, 0, 0, 1]
            ]

        return mat
    }

    /**
    * @description 视角矩阵
    * @param from 源点
    * @param to 目标点
    * @returns  Matrix4 视角矩阵
    */
    public static LookAt(from: Point3, to: Point3, up: Vector3 = new Vector3(0, 0, 1)): Matrix4 {
        let f = from.vectorTo(to).normalize();
        let s = f.cross(up).normalize();
        let u = s.cross(f).normalize();

        let tmp1 = s.x * from.x + s.y * from.y + s.z * from.z;
        let tmp2 = u.x * from.x + u.y * from.y + u.z * from.z;
        let tmp3 = f.x * from.x + f.y * from.y + f.z * from.z;

        let mat = new Matrix4()
        mat._data = [
            [s.x, s.y, s.z, -tmp1],
            [u.x, u.y, u.z, -tmp2],
            [-f.x, -f.y, -f.z, tmp3],
            [0, 0, 0, 1]
        ]
        return mat;
    }

    /**
    * @description 从世界坐标转到局部坐标的变换矩阵
    * @param origin - 局部原点（与世界中，下同）
    * @param xAxi - 局部x轴
    * @param yAxi - 局部y轴
    * @param zAxi - 局部z轴
    * @returns 变换矩阵
    */
    public static ToLocal(origin: Point3, xAxi: Vector3, yAxi: Vector3, zAxi: Vector3): Matrix4 {
        let mat = new Matrix4([[xAxi.x, xAxi.y, xAxi.z, -(origin.x * xAxi.x + origin.y * xAxi.y + origin.z * xAxi.z)],
        [yAxi.x, yAxi.y, yAxi.z, -(origin.x * yAxi.x + origin.y * yAxi.y + origin.z * yAxi.z)],
        [zAxi.x, zAxi.y, zAxi.z, -(origin.x * zAxi.x + origin.y * zAxi.y + origin.z * zAxi.z)],
        [0, 0, 0, 1]]);
        return mat;
    }

    /**
    * @description 克隆当前矩阵
    * @returns 新的矩阵对象
    */
    public clone(): Matrix4 {
        let mat = new Matrix4(this.rawDeepClone());
        return mat;
    }

    /**
    * @description 深拷贝矩阵的原始数据
    * @param data - 要拷贝的数据，如果不提供则使用当前矩阵数据
    * @returns 新的二维数组，包含矩阵数据的深拷贝
    * @private
    */
    private rawDeepClone(data?: number[][]): number[][] {
        const source = data || this._data;
        let raw: number[][] = [[], [], [], []];
        for (let i = 0; i < 4; i++) {
            for (let j = 0; j < 4; j++) {
                raw[i][j] = source[i][j];
            }
        }
        return raw;
    }

    /**
    * @description 矩阵相乘 this X mat
    * @param mat 相乘的矩阵
    * @returns 相乘后的矩阵
    */
    public multiplyMatrix(mat: Matrix4): Matrix4 {
        let newMat = new Matrix4();
        let i: number;
        let j: number;
        for (i = 0; i < 4; i++) {
            for (j = 0; j < 4; j++)
                newMat._data[i][j] =
                    this._data[i][0] * mat._data[0][j] +
                    this._data[i][1] * mat._data[1][j] +
                    this._data[i][2] * mat._data[2][j] +
                    this._data[i][3] * mat._data[3][j];
        }
        return newMat;
    }

    /**
    * @description 矩阵相乘 this X mat，结果存储在自身
    * @param mat - 要相乘的矩阵
    * @returns 当前矩阵对象
    */
    public multiplyMatrixSelf(mat: Matrix4): Matrix4 {
        let newMat = new Matrix4();
        for (let i = 0; i < 4; i++) {
            for (let j = 0; j < 4; j++)
                newMat._data[i][j] =
                    this._data[i][0] * mat._data[0][j] +
                    this._data[i][1] * mat._data[1][j] +
                    this._data[i][2] * mat._data[2][j] +
                    this._data[i][3] * mat._data[3][j];
        }
        this._data = newMat._data
        return this
    }

    /**
    * @description 求逆矩阵
    * @returns 逆矩阵
    */
    public inverse(): Matrix4 {
        const te = this._data,
            n11 = te[0][0], n21 = te[0][1], n31 = te[0][2], n41 = te[0][3],
            n12 = te[1][0], n22 = te[1][1], n32 = te[1][2], n42 = te[1][3],
            n13 = te[2][0], n23 = te[2][1], n33 = te[2][2], n43 = te[2][3],
            n14 = te[3][0], n24 = te[3][1], n34 = te[3][2], n44 = te[3][3],

            t11 = n23 * n34 * n42 - n24 * n33 * n42 + n24 * n32 * n43 - n22 * n34 * n43 - n23 * n32 * n44 + n22 * n33 * n44,
            t12 = n14 * n33 * n42 - n13 * n34 * n42 - n14 * n32 * n43 + n12 * n34 * n43 + n13 * n32 * n44 - n12 * n33 * n44,
            t13 = n13 * n24 * n42 - n14 * n23 * n42 + n14 * n22 * n43 - n12 * n24 * n43 - n13 * n22 * n44 + n12 * n23 * n44,
            t14 = n14 * n23 * n32 - n13 * n24 * n32 - n14 * n22 * n33 + n12 * n24 * n33 + n13 * n22 * n34 - n12 * n23 * n34;

        const det = n11 * t11 + n21 * t12 + n31 * t13 + n41 * t14;

        if (det === 0) return Matrix4.Zero()

        const detInv = 1 / det;

        let mat = new Matrix4()
        let matRaw = mat._data

        matRaw[0][0] = t11 * detInv;
        matRaw[0][1] = (n24 * n33 * n41 - n23 * n34 * n41 - n24 * n31 * n43 + n21 * n34 * n43 + n23 * n31 * n44 - n21 * n33 * n44) * detInv;
        matRaw[0][2] = (n22 * n34 * n41 - n24 * n32 * n41 + n24 * n31 * n42 - n21 * n34 * n42 - n22 * n31 * n44 + n21 * n32 * n44) * detInv;
        matRaw[0][3] = (n23 * n32 * n41 - n22 * n33 * n41 - n23 * n31 * n42 + n21 * n33 * n42 + n22 * n31 * n43 - n21 * n32 * n43) * detInv;

        matRaw[1][0] = t12 * detInv;
        matRaw[1][1] = (n13 * n34 * n41 - n14 * n33 * n41 + n14 * n31 * n43 - n11 * n34 * n43 - n13 * n31 * n44 + n11 * n33 * n44) * detInv;
        matRaw[1][2] = (n14 * n32 * n41 - n12 * n34 * n41 - n14 * n31 * n42 + n11 * n34 * n42 + n12 * n31 * n44 - n11 * n32 * n44) * detInv;
        matRaw[1][3] = (n12 * n33 * n41 - n13 * n32 * n41 + n13 * n31 * n42 - n11 * n33 * n42 - n12 * n31 * n43 + n11 * n32 * n43) * detInv;

        matRaw[2][0] = t13 * detInv;
        matRaw[2][1] = (n14 * n23 * n41 - n13 * n24 * n41 - n14 * n21 * n43 + n11 * n24 * n43 + n13 * n21 * n44 - n11 * n23 * n44) * detInv;
        matRaw[2][2] = (n12 * n24 * n41 - n14 * n22 * n41 + n14 * n21 * n42 - n11 * n24 * n42 - n12 * n21 * n44 + n11 * n22 * n44) * detInv;
        matRaw[2][3] = (n13 * n22 * n41 - n12 * n23 * n41 - n13 * n21 * n42 + n11 * n23 * n42 + n12 * n21 * n43 - n11 * n22 * n43) * detInv;

        matRaw[3][0] = t14 * detInv;
        matRaw[3][1] = (n13 * n24 * n31 - n14 * n23 * n31 + n14 * n21 * n33 - n11 * n24 * n33 - n13 * n21 * n34 + n11 * n23 * n34) * detInv;
        matRaw[3][2] = (n14 * n22 * n31 - n12 * n24 * n31 - n14 * n21 * n32 + n11 * n24 * n32 + n12 * n21 * n34 - n11 * n22 * n34) * detInv;
        matRaw[3][3] = (n12 * n23 * n31 - n13 * n22 * n31 + n13 * n21 * n32 - n11 * n23 * n32 - n12 * n21 * n33 + n11 * n22 * n33) * detInv;

        return mat;
    }

    /**
    * @description 求逆矩阵..
    * @returns 逆矩阵
    */
    public inverseSelf(): Matrix4 {
        const te = this._data,
            n11 = te[0][0], n21 = te[0][1], n31 = te[0][2], n41 = te[0][3],
            n12 = te[1][0], n22 = te[1][1], n32 = te[1][2], n42 = te[1][3],
            n13 = te[2][0], n23 = te[2][1], n33 = te[2][2], n43 = te[2][3],
            n14 = te[3][0], n24 = te[3][1], n34 = te[3][2], n44 = te[3][3],

            t11 = n23 * n34 * n42 - n24 * n33 * n42 + n24 * n32 * n43 - n22 * n34 * n43 - n23 * n32 * n44 + n22 * n33 * n44,
            t12 = n14 * n33 * n42 - n13 * n34 * n42 - n14 * n32 * n43 + n12 * n34 * n43 + n13 * n32 * n44 - n12 * n33 * n44,
            t13 = n13 * n24 * n42 - n14 * n23 * n42 + n14 * n22 * n43 - n12 * n24 * n43 - n13 * n22 * n44 + n12 * n23 * n44,
            t14 = n14 * n23 * n32 - n13 * n24 * n32 - n14 * n22 * n33 + n12 * n24 * n33 + n13 * n22 * n34 - n12 * n23 * n34;

        const det = n11 * t11 + n21 * t12 + n31 * t13 + n41 * t14;

        if (det === 0) {
            this._data = Matrix4.Zero()._data
            return this
        }

        const detInv = 1 / det;

        this._data[0][0] = t11 * detInv;
        this._data[0][1] = (n24 * n33 * n41 - n23 * n34 * n41 - n24 * n31 * n43 + n21 * n34 * n43 + n23 * n31 * n44 - n21 * n33 * n44) * detInv;
        this._data[0][2] = (n22 * n34 * n41 - n24 * n32 * n41 + n24 * n31 * n42 - n21 * n34 * n42 - n22 * n31 * n44 + n21 * n32 * n44) * detInv;
        this._data[0][3] = (n23 * n32 * n41 - n22 * n33 * n41 - n23 * n31 * n42 + n21 * n33 * n42 + n22 * n31 * n43 - n21 * n32 * n43) * detInv;

        this._data[1][0] = t12 * detInv;
        this._data[1][1] = (n13 * n34 * n41 - n14 * n33 * n41 + n14 * n31 * n43 - n11 * n34 * n43 - n13 * n31 * n44 + n11 * n33 * n44) * detInv;
        this._data[1][2] = (n14 * n32 * n41 - n12 * n34 * n41 - n14 * n31 * n42 + n11 * n34 * n42 + n12 * n31 * n44 - n11 * n32 * n44) * detInv;
        this._data[1][3] = (n12 * n33 * n41 - n13 * n32 * n41 + n13 * n31 * n42 - n11 * n33 * n42 - n12 * n31 * n43 + n11 * n32 * n43) * detInv;

        this._data[2][0] = t13 * detInv;
        this._data[2][1] = (n14 * n23 * n41 - n13 * n24 * n41 - n14 * n21 * n43 + n11 * n24 * n43 + n13 * n21 * n44 - n11 * n23 * n44) * detInv;
        this._data[2][2] = (n12 * n24 * n41 - n14 * n22 * n41 + n14 * n21 * n42 - n11 * n24 * n42 - n12 * n21 * n44 + n11 * n22 * n44) * detInv;
        this._data[2][3] = (n13 * n22 * n41 - n12 * n23 * n41 - n13 * n21 * n42 + n11 * n23 * n42 + n12 * n21 * n43 - n11 * n22 * n43) * detInv;

        this._data[3][0] = t14 * detInv;
        this._data[3][1] = (n13 * n24 * n31 - n14 * n23 * n31 + n14 * n21 * n33 - n11 * n24 * n33 - n13 * n21 * n34 + n11 * n23 * n34) * detInv;
        this._data[3][2] = (n14 * n22 * n31 - n12 * n24 * n31 - n14 * n21 * n32 + n11 * n24 * n32 + n12 * n21 * n34 - n11 * n22 * n34) * detInv;
        this._data[3][3] = (n12 * n23 * n31 - n13 * n22 * n31 + n13 * n21 * n32 - n11 * n23 * n32 - n12 * n21 * n33 + n11 * n22 * n33) * detInv;

        return this;
    }

    /**
    * @description 分解矩阵
    * @param orientationStyle 方向样式
    * @returns  { position: Vector3, rotation: Vector4, scale: Vector3 }
    */
    public decompose(orientationStyle: string = "eulerAngles"): { position: Vector3, rotation: Vector4, scale: Vector3 } {
        if (orientationStyle == null) {
            orientationStyle = "eulerAngles";
        }
        let m = this.clone();
        let mr = convertRaw(m.raw);
        let pos = new Vector3(mr[12], mr[13], mr[14]);
        mr[12] = 0;
        mr[13] = 0;
        mr[14] = 0;
        let scale = new Vector3();
        scale.x = Math.sqrt(mr[0] * mr[0] + mr[1] * mr[1] + mr[2] * mr[2]);
        scale.y = Math.sqrt(mr[4] * mr[4] + mr[5] * mr[5] + mr[6] * mr[6]);
        scale.z = Math.sqrt(mr[8] * mr[8] + mr[9] * mr[9] + mr[10] * mr[10]);
        if (mr[0] * (mr[5] * mr[10] - mr[6] * mr[9]) - mr[1] * (mr[4] * mr[10] - mr[6] * mr[8]) + mr[2] * (mr[4] * mr[9] - mr[5] * mr[8]) < 0) {
            scale.z = -scale.z;
        }
        if (scale.x != 0) {
            mr[0] /= scale.x;
            mr[1] /= scale.x;
            mr[2] /= scale.x;
        }
        if (scale.y != 0) {
            mr[4] /= scale.y;
            mr[5] /= scale.y;
            mr[6] /= scale.y;
        }
        if (scale.z != 0) {
            mr[8] /= scale.z;
            mr[9] /= scale.z;
            mr[10] /= scale.z;
        }
        let rot = new Vector4(0, 0, 0, 1);
        switch (orientationStyle) {
            case "axisAngle":
                rot.w = Math.acos((mr[0] + mr[5] + mr[10] - 1) / 2);
                let len = Math.sqrt((mr[6] - mr[9]) * (mr[6] - mr[9]) + (mr[8] - mr[2]) * (mr[8] - mr[2]) + (mr[1] - mr[4]) * (mr[1] - mr[4]));
                if (len != 0) {
                    rot.x = (mr[6] - mr[9]) / len;
                    rot.y = (mr[8] - mr[2]) / len;
                    rot.z = (mr[1] - mr[4]) / len;
                } else {
                    rot.x = rot.y = rot.z = 0;
                }
                break;
            case "eulerAngles":
                rot.y = Math.asin(-mr[2]);
                if (mr[2] != 1 && mr[2] != -1) {
                    rot.x = Math.atan2(mr[6], mr[10]);
                    rot.z = Math.atan2(mr[1], mr[0]);
                } else {
                    rot.z = 0;
                    if (mr[2] == 1) {
                        rot.x = -Math.atan2(mr[4], mr[5]);
                    } else {
                        rot.x = Math.atan2(mr[4], mr[5]);
                    }
                }
                break;
            case "quaternion":
                let tr = mr[0] + mr[5] + mr[10];
                if (tr > 0) {
                    let rotw4 = 2 * Math.sqrt(1 + tr);
                    rot.w = rotw4 / 4;
                    rot.x = (mr[6] - mr[9]) / rotw4;
                    rot.y = (mr[8] - mr[2]) / rotw4;
                    rot.z = (mr[1] - mr[4]) / rotw4;
                } else if (mr[0] > mr[5] && mr[0] > mr[10]) {
                    let rotx4 = 2 * Math.sqrt(1 + mr[0] - mr[5] - mr[10]);
                    rot.x = rotx4 / 4;
                    rot.w = (mr[6] - mr[9]) / rotx4;
                    rot.y = (mr[1] + mr[4]) / rotx4;
                    rot.z = (mr[8] + mr[2]) / rotx4;
                } else if (mr[5] > mr[10]) {
                    let roty4 = 2 * Math.sqrt(1 + mr[5] - mr[0] - mr[10]);
                    rot.y = roty4 / 4;
                    rot.x = (mr[1] + mr[4]) / roty4;
                    rot.w = (mr[8] - mr[2]) / roty4;
                    rot.z = (mr[6] + mr[9]) / roty4;
                } else {
                    let rotz4 = 2 * Math.sqrt(1 + mr[10] - mr[0] - mr[5]);
                    rot.z = rotz4 / 4;
                    rot.x = (mr[8] + mr[2]) / (4 * rot.z);
                    rot.y = (mr[6] + mr[9]) / (4 * rot.z);
                    rot.w = (mr[1] - mr[4]) / (4 * rot.z);
                }
                break;
        }
        return { position: pos, rotation: rot, scale: scale };

        function convertRaw(raw: number[][]): number[] {
            //这只是把列行式转成行列式
            let arr = [];
            for (let i = 0; i < 4; i++) {
                for (let j = 0; j < 4; j++) {
                    arr.push(raw[j][i]);// 验证行列序是否需要变换
                }
            }
            return arr;
        }
    }

    /**
    * @description 转置
    * @returns 转置后的矩阵
    */
    public transpose(): Matrix4 {
        let mat = new Matrix4();
        let raw = this.rawDeepClone();
        let i: number;
        let j: number;

        for (i = 0; i < 4; i++) {
            for (j = i; j < 4; j++) {
                [raw[i][j], raw[j][i]] = [raw[j][i], raw[i][j]];
            }
        }
        mat._data = raw
        return mat
    }

    /**
    * @description 转置，改变自身
    * @returns 转置后的矩阵
    */
    public transposeSelf(): Matrix4 {
        let raw = this._data;
        let i: number;
        let j: number;

        for (i = 0; i < 4; i++) {
            for (j = i; j < 4; j++) {
                [raw[i][j], raw[j][i]] = [raw[j][i], raw[i][j]];
            }
        }
        return this
    }

    /**
    * @description 矩阵与点相乘, [Mat] X [pt]
    * @param point 点
    * @returns 相乘后的点
    */
    public multiplyPoint(point: Point3): Point3 {
        let data = this._data;
        let rlt = new Point3();
        rlt.x = data[0][0] * point.x + data[0][1] * point.y + data[0][2] * point.z + data[0][3];
        rlt.y = data[1][0] * point.x + data[1][1] * point.y + data[1][2] * point.z + data[1][3];
        rlt.z = data[2][0] * point.x + data[2][1] * point.y + data[2][2] * point.z + data[2][3];
        return rlt;
    }

    /**
    * @description 矩阵与向量相乘，即t（第4列）不计考量
    * @param vec 向量
    * @returns 相乘后的向量
    */
    public multiplyVector(vec: Vector3): Vector3 {
        let data = this._data;
        let rlt = new Vector3(0, 0, 0);
        rlt.x = data[0][0] * vec.x + data[0][1] * vec.y + data[0][2] * vec.z;
        rlt.y = data[1][0] * vec.x + data[1][1] * vec.y + data[1][2] * vec.z;
        rlt.z = data[2][0] * vec.x + data[2][1] * vec.y + data[2][2] * vec.z;
        return rlt;
    }

    /**
    * @description 获取矩阵列
    * @param colIx - 列索引（第一列:0, 第二列1, 第三列2）
    * @returns  Vector3
    */
    public getColumn(result: Vector3, colIx: number): Vector3 {
        if (colIx < 0 || colIx >= 4) {
            throw new Error("Column index out of bounds");
        }
        let data = this._data;
        result.coord.set(
            data[0][colIx],
            data[1][colIx],
            data[2][colIx]
        );
        return result;
    }

    /**
    * @description 获取矩阵行
    * @param result - 结果向量
    * @param rowIx - 行索引（第一行:0, 第二行:1, 第三行:2, 第四行:3）
    * @returns 行向量
    */
    public getRow(result: Vector3, rowIx: number): Vector3 {
        if (rowIx < 0 || rowIx >= 4) {
            throw new Error("Row index out of bounds");
        }
        let data = this._data;
        result.coord.set(
            data[rowIx][0],
            data[rowIx][1],
            data[rowIx][2]
        );
        return result;
    }

    /**
    * @description 从源复制矩阵数据
    * @param src - 源矩阵
    * @returns 当前矩阵对象
    */
    public copyFrom(src: Matrix4): Matrix4 {
        const tRaw = this._data;
        const sRaw = src._data;
        for (let i = 0; i < 4; i++) {
            for (let j = 0; j < 4; j++) {
                tRaw[i][j] = sRaw[i][j];
            }
        }
        return this;
    }
}
